#include "QuadTreeNode.h"
#define MAX_DEPTH 5

QuadTreeNode::QuadTreeNode(QuadTreeNode* parent, BoundingBox bBox, OctTree* tree):
	_parent(parent),
	_box(bBox),
	_tree(tree),
	Depth(0)
{
	if(_parent != NULL)
	{
		Depth = _parent->Depth + 1;
	}
	for(int i = 0; i < 8; i++)
	{
		_children[i] = NULL;
	}
}


QuadTreeNode::~QuadTreeNode(void)
{
	// parent sholud never have to be deleted it should delete itself
	delete [] _children; 
}

void QuadTreeNode::MakeNodes()
{
	float halfX = _box.Width()/2;
	float halfY = _box.Height()/2;
	float halfZ = _box.Depth()/2;
	
	Vector3<float> mins = _box.Mins();
	Vector3<float> maxes = _box.Maxes();

	BoundingBox(mins.X(),			mins.Y(),			 mins.Z(),
				mins.X() + halfX,	mins.Y() + halfY,	 mins.Z()+ halfZ); 

	_children[0] = new QuadTreeNode(this,BoundingBox(mins.X(),			mins.Y(),			 mins.Z(),
													mins.X() + halfX,	mins.Y() + halfY,	 mins.Z()+ halfZ), _tree);

	_children[1] = new QuadTreeNode(this,BoundingBox(mins.X() + halfX,	mins.Y(),			 mins.Z(),
													maxes.X(),			mins.Y() + halfY,	 mins.Z()+ halfZ), _tree);

	_children[2] = new QuadTreeNode(this,BoundingBox(mins.X(),			mins.Y() + halfY,	 mins.Z(),
													mins.X() + halfX,	maxes.Y(),			 mins.Z()+ halfZ), _tree);

	_children[3] = new QuadTreeNode(this,BoundingBox(mins.X() + halfX,	mins.Y() + halfY,	 mins.Z(),
													maxes.X(),			maxes.Y(),			 mins.Z()+ halfZ), _tree);

	_children[4] = new QuadTreeNode(this,BoundingBox(mins.X(),			mins.Y(),			 mins.Z() + halfZ,
													mins.X() + halfX,	mins.Y() + halfY,	 maxes.Z()), _tree);

	_children[5] = new QuadTreeNode(this,BoundingBox(mins.X() + halfX,	mins.Y(),			 mins.Z() + halfZ,
													maxes.X(),			mins.Y() + halfY,	 maxes.Z()), _tree);

	_children[6] = new QuadTreeNode(this,BoundingBox(mins.X(),			mins.Y() + halfY,	 mins.Z() + halfZ,
													mins.X() + halfX,	maxes.Y(),			 maxes.Z()), _tree);

	_children[7] = new QuadTreeNode(this,BoundingBox(mins.X() + halfX,	mins.Y() + halfY,	 mins.Z() + halfZ,
													maxes.X(),			maxes.Y(),			 maxes.Z()), _tree);

}

std::vector<Colider*> QuadTreeNode::InBox()
{
	std::vector<Colider*> in;
	if(_parent != NULL)
	{
		 in = _parent->InBox();
	}
	for(int i = 0 ; i < _objects.size(); i++)
	{
		in.push_back(_objects[i]->_gameObject);
	}
	return in;
}

QuadTreeNode* QuadTreeNode::AddObject(OctChild* obj)
{
	if(_children[0] == NULL)
	{
		if(_box.Contains(obj->_gameObject->BBox()))
		{
			_objects.push_back(obj);
			return this;
		}
		else
		{
			return NULL;
		}
	}
	else
	{
		for(int i = 0; i < 8; i++)
		{
			QuadTreeNode* end = _children[i]->AddObject(obj);
			if(end != NULL)
			{
				return end;
			}
		}
		if(_box.Contains(obj->_gameObject->BBox()))
		{
			_objects.push_back(obj);
			return this;
		}
		else
		{
			return NULL;
		}

	}
	return this;
}

void QuadTreeNode::RemoveObject(OctChild* obj)
{
	std::vector<OctChild*>::iterator itor; 
	for(itor = _objects.begin(); itor != _objects.end(); itor++)
	{
		if(obj == (*itor))
		{
			_objects.erase(itor);
			return;
		}
	}
}

int QuadTreeNode::Contains()
{
	int sum = _objects.size();
	if(_children[0] != NULL)
	{
		for(int i = 0; i < 8; i++)
		{
			sum += _children[i]->Contains(); 
		}
	}
	return sum;
}
// take the nodes that are contatined in this node and check if they can be moved to a lower or uper level
void QuadTreeNode::Orginize()
{
	std::vector<OctChild*>::iterator itor; 
	for(itor = _objects.begin(); itor != _objects.end(); )
	{
		if(_box.Contains((*itor)->_gameObject->BBox()))
		{
			if(this->Contains() > 3)
			{
				if( MoveDown((*itor)))
				{
					_objects.erase(itor);
					itor = _objects.begin();
				}
				else
				{
					itor++;
				}
			}
			else
			{
				break;
			}
		}
		else
		{
			if(_parent != NULL)
			{
				if(_parent->MoveUp((*itor)))
				{
					_objects.erase(itor);
					_parent->Orginize();
				}
				itor = _objects.begin();
			}
			else
			{
				itor++;
			}
		}
	}
	
}

bool QuadTreeNode::MoveUp(OctChild* obj)
{
	if(_box.Contains(obj->_gameObject->BBox()))
	{
		_objects.push_back(obj);
		obj->_location = this;
		return true;
	}
    else
	{
		if(_parent != NULL)
		{
			return _parent->MoveUp(obj);
		}
		else
		{
			_tree->removeGameObject(obj->_gameObject);
			return false;
			//throw "Out of bounds error, outside of oct tree";
		}
	}

}

bool QuadTreeNode::MoveDown(OctChild* obj)
{
	if(_children[0] == NULL)
	{
		if(Depth < MAX_DEPTH)
		{		
			MakeNodes();
		}
		else
		{
			return false;
		}
	}
	for(int i = 0; i < 8; i++)
	{
		QuadTreeNode* qtn = _children[i]->AddObject(obj);
		if(qtn != NULL)
		{
			obj->_location = qtn;
			//_children[i]->Orginize();
			return true;
		}
	}
	return false;
}


// draw the actural node and is sub nodes that contain objects
void QuadTreeNode::Draw()
{
	if(this->Contains() > 0)
	{
		if(_objects.size() > 0)
		_box.Draw();

		for(int i = 0; i < 8; i++)
		{
			if(_children[i] != NULL)
			{
				_children[i]->Draw();
			}
			else
			{
				break;
			}
		}
	}
}
